
function missile_trajectory_optimization()
    % 设定目标状态 (目标点位置与速度)
    x_T = 1000; % 目标x位置
    y_T = 1000; % 目标y位置
    z_T = 1000; % 目标z位置
    vx_T = 0;   % 目标vx速度
    vy_T = 0;   % 目标vy速度
    vz_T = 0;   % 目标vz速度

    % 初始猜测
    P_0 = [100, 100, 100, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0]; % 初始条件（包括初始位置、速度、四元数、角速度）

    % 设定时间参数
    t_f = 100; % 最终时间 (比如 100秒)
    dt = 0.1;  % 时间步长

    % 迭代求解
    tolerance = 1e-6; % 误差容忍度
    max_iter = 50;    % 最大迭代次数

    P = P_0; % 初始猜测

    for k = 1:max_iter
        % 计算导弹轨迹
        [t, X] = solve_trajectory(P, t_f, dt); % solve_trajectory是求解轨迹的函数
        % 获取最后的状态 (位置与速度)
        final_state = X(end, :);
        
        % 计算误差
        E = final_state(1:6) - [x_T, y_T, z_T, vx_T, vy_T, vz_T];
        
        % 如果误差足够小，说明收敛
        if norm(E) < tolerance
            fprintf('收敛，找到合适的初始条件\n');
            disp(P);
            break;
        end
        
        % 计算雅可比矩阵（利用有限差分法）
        J = compute_jacobian(P, t_f, dt, E); % compute_jacobian是计算雅可比矩阵的函数

        % 更新初始条件
        P = P - J \ E'; % Newton-Raphson更新

        % 输出迭代信息
        fprintf('迭代次数：%d, 当前误差：%f\n', k, norm(E));
    end
end

% 轨迹求解函数
function [t, X] = solve_trajectory(P, t_f, dt)
    % 从初始条件 P 中提取状态变量
    x0 = P(1); y0 = P(2); z0 = P(3);
    vx0 = P(4); vy0 = P(5); vz0 = P(6);
    q0 = P(7); q1 = P(8); q2 = P(9); q3 = P(10);
    omega_x = P(11); omega_y = P(12); omega_z = P(13);

    % 设置初始状态
    X0 = [x0, y0, z0, vx0, vy0, vz0, q0, q1, q2, q3, omega_x, omega_y, omega_z];
    t = 0:dt:t_f; % 时间向量
    num_steps = length(t); % 步数

    % 初始化状态变量矩阵
    X = zeros(num_steps, 13);
    X(1, :) = X0; % 初始状态

    % 迭代解动力学方程（四阶Runge-Kutta方法）
    for i = 1:num_steps-1
        k1 = dynamics(X(i, :));
        k2 = dynamics(X(i, :) + 0.5*dt*k1');
        k3 = dynamics(X(i, :) + 0.5*dt*k2');
        k4 = dynamics(X(i, :) + dt*k3');
        X(i+1, :) = X(i, :) + (dt/6)*(k1' + 2*k2' + 2*k3' + k4');
    end
end

% 动力学方程
function dxdt = dynamics(X)
    % 从状态中提取各个变量
    x = X(1); y = X(2); z = X(3);
    vx = X(4); vy = X(5); vz = X(6);
    q0 = X(7); q1 = X(8); q2 = X(9); q3 = X(10);
    omega_x = X(11); omega_y = X(12); omega_z = X(13);

    % 空气动力学力（假设给定的输入力矩）
    Fxb = 0; Fxd = 0; Fyb = 0; Fyd = 0; Fzb = 0; Fzd = 0; % 需要根据具体模型给定
    m = 1; % 质量，假设为1kg

    % 计算速度和位置变化率
    dxdt = zeros(13, 1);
    dxdt(1) = vx; % x方向速度
    dxdt(2) = vy; % y方向速度
    dxdt(3) = vz; % z方向速度
    
    % 加速度（动力学方程）
    dxdt(4) = (- (Fxb + Fxd) * (2*q2^2 + 2*q3^2 - 1) + (2*q0*q3 - 2*q1*q2) * (Fyb + Fyd) - (2*q0*q2 + 2*q1*q3) * (Fzb + Fzd)) / m;
    dxdt(5) = -9.81 - (Fyb + Fyd) * (2*q1^2 + 2*q3^2 - 1) - (2*q0*q3 + 2*q1*q2) * (Fxb + Fxd) + (2*q0*q1 - 2*q2*q3) * (Fzb + Fzd) / m;
    dxdt(6) = (- (Fzb + Fzd) * (2*q1^2 + 2*q2^2 - 1) + (2*q0*q2 - 2*q1*q3) * (Fxb + Fxd) - (2*q0*q1 + 2*q2*q3) * (Fyb + Fyd)) / m;

    % 四元数更新（姿态角速度）
    dxdt(7) = -0.5 * (omega_x * q1 + omega_y * q2 + omega_z * q3);
    dxdt(8) = 0.5 * (omega_x * q0 - omega_y * q3 + omega_z * q2);
    dxdt(9) = 0.5 * (omega_y * q0 + omega_x * q3 - omega_z * q1);
    dxdt(10) = 0.5 * (omega_z * q0 - omega_x * q2 + omega_y * q1);

    % 角速度变化率（简化假设，无力矩）
    dxdt(11) = 0; % 角速度x方向变化率
    dxdt(12) = 0; % 角速度y方向变化率
    dxdt(13) = 0; % 角速度z方向变化率
end

% 计算雅可比矩阵（有限差分）
function J = compute_jacobian(P, t_f, dt, E)
    % 设定目标状态 (目标点位置与速度)
    x_T = 1000; % 目标x位置
    y_T = 1000; % 目标y位置
    z_T = 1000; % 目标z位置
    vx_T = 0;   % 目标vx速度
    vy_T = 0;   % 目标vy速度
    vz_T = 0;   % 目标vz速度
    delta = 1e-6; % 差分步长
    num_params = length(P);
    J = zeros(length(E), num_params);

    for i = 1:num_params
        P_plus = P;
        P_plus(i) = P_plus(i) + delta; % 增加差分量

        % 计算增量误差
        [~, X_plus] = solve_trajectory(P_plus, t_f, dt);
        final_state_plus = X_plus(end, :);
        E_plus = final_state_plus(1:6) - [x_T, y_T, z_T, vx_T, vy_T, vz_T];

        % 差分计算
        J(:, i) = (E_plus' - E) / delta;
    end
end
